#include "trajPlanner.h"


static float getDistance(Point* p1, Point* p2){
    float dist = sqrtf(pow((p1->x - p2->x), 2) 
                        + pow((p1->y - p2->y), 2) 
                        + pow((p1->z - p2->z), 2));
    return dist;
}

//Linear position and quaterion interpolation algorithm
int linearPlanner(Pose* start_pose, Pose* end_pose, Pose* way_points, int max_num){
    static float step = 10.0; //interpolation accuracy: 10mm
    int points_num = 0;
    Quaterion start_quat, end_quat;
    Point start_point, end_point;
    getQuatFromPose(start_pose, &start_quat);
    getQuatFromPose(end_pose, &end_quat);
    getPointFromPose(start_pose, &start_point);
    getPointFromPose(end_pose, &end_point);
    float dist = getDistance(&start_point, &end_point);
    int cnt = (dist / step) + 1;
    if(cnt > max_num){
        cnt = max_num;
    }
    float t;
    Quaterion temp_quat;
    Point temp_point;
    Pose temp_pose;
    int i;
    for(i = 0; i <= cnt; i++){
        t = (1.0*i) / cnt;
        quaternionSlerp(&start_quat, &end_quat, t, &temp_quat);
        getMiddlePoint(&start_point, &end_point, t, &temp_point);
        quatPoint2Pose(&temp_quat, &temp_point, &temp_pose);
        ++points_num;
        way_points[i].x = temp_pose.x;
        way_points[i].y = temp_pose.y;
        way_points[i].z = temp_pose.z;
        way_points[i].qx = temp_pose.qx;
        way_points[i].qy = temp_pose.qy;
        way_points[i].qz = temp_pose.qz;
        way_points[i].qw = temp_pose.qw;
    }
    return points_num;
}

// TODO
int arcPlanner(Pose* start_pose, Pose* mid_pose, Pose* end_pose, Pose* way_points, int max_num){
    return 0;
}